#include <Wire.h>
#include <Servo.h>
#include <EEPROM.h>
#include <Depecabot3.h>

Depecabot robot;

char cadena[10],*joy_x_cad,*joy_y_cad; //Buffer y punteros a las subcadenas
int joy_x,joy_y;
int distancia,bateria;
long timer_envio;

void setup() 
{
  robot.init ();

  robot.lcd.clear();

  robot.servo1.attach(SERVO);            //Inicializacion servo
  robot.servo1.write(90);
}

void loop() 
{
  int i,m1,m2;

  if(millis()-timer_envio>200)  //Cada 200ms lee al sensor y la bateria y lo enva por el puerto serie.
  {
    timer_envio=millis();
    
    distancia=robot.gp2ymm(GP2);
    Serial1.print(distancia);
    
    bateria=robot.bateria()*1000;  //La bateria se envia en mV para facilitar la conversion
    Serial1.print(" ");
    Serial1.println(bateria);
  }

  if(Serial1.available())    //Si hay datos en el puerto
  {
    Serial1.readBytesUntil(13,cadena,10);	//Lee hasta encontrar un cambio de linea o un maximo de 10 caracteres

    robot.lcd.setCursor(0,0);  //Escribe el contenido del buffer de recepcion
    robot.lcd.print(cadena);
    robot.lcd.print("  ");

    joy_x_cad=strtok(cadena," ");  //Obtiene el primer dato, separado por un espacio
    joy_y_cad=strtok(NULL," ");  //Obtiene el segundo dato

    joy_x=atoi(joy_x_cad);    //Convierte los datos a enteros
    joy_y=atoi(joy_y_cad);

    robot.motoresLinealAngular(joy_y,joy_x);

    robot.lcd.setCursor(0,2);  //Escribe los datos ya separados y en entero
    robot.lcd.print(joy_x);
    robot.lcd.print("  ");
    robot.lcd.setCursor(0,3);
    robot.lcd.print(joy_y);
    robot.lcd.print("  ");

    for(i=0;i<10;i++)
      cadena[i]=0;      //Borra la cadena para evitar problemas con el strtok
  }
}

//Programa para processing
//Pegar en un sketch de processing, editar el puerto serie bluetooth y ejecutar

/*
import processing.serial.*;

Serial myPort;      // The serial port
int joy_size=270, joy_x=320, joy_y=180;
int joy_x_min, joy_y_min, joy_x_max, joy_y_max;
int v_max=200;
int distancia;
float bateria;
String[] datos;

void setup() 
{
  size(640, 360); 

  joy_x_min=joy_x-joy_size/2;    //Calcula las coordenadas de la caja del joystick
  joy_y_min=joy_y-joy_size/2;
  joy_x_max=joy_x+joy_size/2;
  joy_y_max=joy_y+joy_size/2;

  dibujaJoystick();    //Dibuja la pantalla por primera vez

  myPort = new Serial(this, "/dev/ttyS99", 115200);  //Conexion al puerto serie, editar con el nombre del puerto deseado

  textFont(createFont("Georgia", 24));
}

void draw() 
{   
  if (mousePressed && mouseX>joy_x_min && mouseX<joy_x_max && mouseY>joy_y_min && mouseY<joy_y_max)  //Si se pulsa el boton del raton y el cursor está dentro de la caja
    {
      myPort.write(((int)(((float)(mouseX-joy_x)/(joy_size/2))*v_max))+" "+((int)(-((float)(mouseY-joy_y)/(joy_size/2))*v_max)));  //Imprime las coordenadas dentro de la caja, escaladas a la velocidad maxima
      myPort.write(13);  //Cambio de linea

      dibujaJoystick();    //Dibuja el joystick

      fill(255, 0, 0);      //Dibuja un circulo rojo que sigue al cursor
      noStroke();
      ellipse(mouseX, mouseY, 20, 20);
  }
  else
  {
    myPort.write("0 0"+"\n");  //Si no se pulsa o el cursor esta fuera de la caja para los motores
    myPort.write(13); //Cambio de linea

    dibujaJoystick();
  }
  noStroke();

  fill(255, 0, 0);          //Barra del sensor de distancia
  rect(70, 75, 20, 240);
  fill(255);
  rect(70, 75+240-((float)distancia/800)*240, 20, ((float)distancia/800)*240);

  if (distancia<200)      //Texto del sensor de distancia
    fill(255, 0, 0);
  else
    fill(255);
  text(distancia, 60, 60);

  if (bateria<6.0)      //Texto del medidor de bateria
    fill(255, 0, 0);
  else
    fill(255);
  text(bateria + "V", 510, 60);
}

void serialEvent(Serial myPort)     //Cuando recibe del puerto
{
  String inBuffer = myPort.readStringUntil(10);    //Lee hasta encontrar un cambio de linea

  if (inBuffer != null ) 
  {
    datos=splitTokens(inBuffer, " ");    //Separa la cadena por los espacios
    if (datos.length==2)    //Si se han recibido dos datos
    {
      distancia=int(trim(datos[0]));    //Convierte a enteros
      bateria=int(trim(datos[1]));
      bateria=bateria/1000;          //Convierte a Voltios (float)

      print(distancia);    //Imprime en la consola de processing
      print("mm - ");
      print(bateria);
      println("V");
    }
  }
}

void dibujaJoystick()
{
  background(51);     //Pone el fondo gris

  fill(255);
  noStroke();
  rect(joy_x_min, joy_y_min, joy_size, joy_size);  //Cuadrado blanco del joystick

  stroke(0);
  line(joy_x_min+(joy_size/2)-20, joy_y_min+(joy_size/2), joy_x_min+(joy_size/2)+20, joy_y_min+(joy_size/2));  //Dibuja la cruz en el centro del joystick
  line(joy_x_min+(joy_size/2), joy_y_min+(joy_size/2)-20, joy_x_min+(joy_size/2), joy_y_min+(joy_size/2)+20);
}
*/













